#include "slam_ros/utilities.h"
#include <ctime>
#include <iomanip>
#include "yaml-cpp/yaml.h"
#include <openssl/md5.h>
#include "rclcpp/rclcpp.hpp"

namespace slam_ros {
bool fileExists(const std::string& file_name) {
  return (access(file_name.c_str(), F_OK) != -1);
}

bool dirExists(const std::string& dir_name) {
  const int ret = access(dir_name.c_str(), 0);

  if (0 == ret) {
    return true;
  } else {
    return false;
  }
}

int doMkdir(const std::string& path, mode_t mode) {
  struct stat st;
  int status = 0;

  if (stat(path.c_str(), &st) != 0) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "Create path: " << path);
    /* Directory does not exist. EEXIST for race condition */
    int ret = mkdir(path.c_str(), mode);
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "ret: " << ret);
    if (ret != 0 && errno != EEXIST) {
      status = -1;
    }
  } else if (!S_ISDIR(st.st_mode)) {
    errno = ENOTDIR;
    status = -1;
  }

  return (status);
}

int mkPath(const std::string& path, mode_t mode) {
  char* pp;
  char* sp;
  int status;
  char* copypath = strdup(path.c_str());

  status = 0;
  pp = copypath;
  while (status == 0 && (sp = strchr(pp, '/')) != 0) {
    if (sp != pp) {
      /* Neither root nor double slash in path */
      *sp = '\0';
      status = doMkdir(copypath, mode);
      *sp = '/';
    }
    pp = sp + 1;
  }
  if (status == 0) {
    std::string path_copy = path + '/';
    status = doMkdir(path_copy, mode);
  }
  free(copypath);

  return (status);
}

void deleteFolderTree(const std::string& dir_name) {
  DIR* dp;
  struct dirent* ep;
  char p_buf[512] = {0};

  if (dir_name == "." || dir_name == "..") {
    return;
  }

  dp = opendir(dir_name.c_str());

  while ((ep = readdir(dp)) != NULL) {
    if (strcmp(ep->d_name, ".") == 0 || strcmp(ep->d_name, "..") == 0) {
      continue;
    }

    sprintf(p_buf, "%s/%s", dir_name.c_str(), ep->d_name);
    if (isDirectory(p_buf)) {
      deleteFolderTree(p_buf);
    } else {
      unlink(p_buf);
      RCLCPP_INFO_STREAM(rclcpp::get_logger("slam_node"), "delete file: " << p_buf);
    }
  }

  closedir(dp);
  rmdir(dir_name.c_str());
}

int directoryFileCount(const std::string& dir_name) {
  DIR* dp;
  struct dirent* ep;
  char p_buf[512] = {0};

  if (dir_name == "." || dir_name == "..") {
    return 0;
  }

  dp = opendir(dir_name.c_str());

  int count = 0;
  while ((ep = readdir(dp)) != NULL) {
    if (strcmp(ep->d_name, ".") == 0 || strcmp(ep->d_name, "..") == 0) {
      continue;
    }

    sprintf(p_buf, "%s/%s", dir_name.c_str(), ep->d_name);
    count++;
  }

  closedir(dp);
  return count;
}

int directoryGetAllFileName(const std::string& dir_name,
                            std::vector<std::string>* file_name) {
  DIR* dp;
  struct dirent* ep;
  auto& all_file = *file_name;
  all_file.clear();
  if (dir_name == "." || dir_name == "..") {
    return -1;
  }
  dp = opendir(dir_name.c_str());
  if(dp == NULL){
    return -1;
  }
  int count = 0;
  while ((ep = readdir(dp)) != NULL) {
    if (ep->d_type == DT_REG) {
      char* extension = strrchr(ep->d_name, '.');

      if (extension != NULL && strcmp(extension, ".png") == 0) {
        all_file.emplace_back(ep->d_name);
        count ++;
      }
    }
  }
  closedir(dp);
  return count;
}

int isDirectory(const std::string& path) {
  struct stat s_buf;

  if (stat(path.c_str(), &s_buf)) {
    return 0;
  }

  return S_ISDIR(s_buf.st_mode);
}

bool getEnvironmentVariable(std::string& str,
                            const char* environment_variable) {
  char* env_var_cstr = nullptr;
  env_var_cstr = std::getenv(environment_variable);
  if (env_var_cstr) {
    str = std::string(env_var_cstr);
    return true;
  } else {
    return false;
  }
}

bool getFileTime(const std::string& file_name, std::string* file_time_info) {
  if (access(file_name.c_str(), 0) == -1) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "File " << file_name << " does not exist!");
    return false;
  }
  FILE* fp;
  int fd;
  struct stat buf;
  fp = fopen(file_name.c_str(), "r");
  fd = fileno(fp);
  fstat(fd, &buf);
  // latest modification time (seconds passed from
  // 01/01/00:00:00 1970 UTC)
  const std::time_t modify_time = buf.st_mtime;
  fclose(fp);
  struct std::tm current_time = {0};
  std::stringstream string_name;
  string_name << std::put_time(localtime_r(&modify_time, &current_time),
                               "%Y-%m-%d %H:%M:%S");
  *file_time_info = string_name.str();
  return true;
}

void RenameFileWithTime(const std::vector<std::string>& files_name) {
  for (const auto& item : files_name) {
    if (!fileExists(item)) continue;
    std::string new_name;
    if (getFileTime(item, &new_name)) {
      new_name = item + "." + new_name;
      rename(item.c_str(), new_name.c_str());
    }
  }
}

bool LoadDataFromTxt(const std::string& file_name, const std::string& delimiter,
                     std::vector<std::vector<std::string>>* all_str_ptr) {
  std::fstream txt_file(file_name);
  if (!txt_file.is_open()) {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("slam_node"),  "Open node_file failed!");
    return false;
  }

  std::vector<std::string> line_str;
  std::string line;
  while (getline(txt_file, line)) {
    line_str.push_back(line);
  }
  txt_file.close();

  if (line_str.empty()) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "File reader: Empty txt file!");
    return false;
  }
  auto& all_str = *all_str_ptr;
  all_str.clear();

  for (const auto& item : line_str) {
    std::vector<std::string> result;
    SplitString(item, delimiter, &result);
    all_str.emplace_back(result);
  }
  if (all_str.empty()) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "File reader: Empty file: " << file_name);
    return false;
  }

  return true;
}

void SplitString(const std::string& str, const std::string& delimiter,
                 std::vector<std::string>* result) {
  std::string::size_type pos;
  auto& split_result = *result;
  split_result.clear();
  const auto split_str = str + delimiter;
  const int size = split_str.size();
  for (int i = 0; i < size; i++) {
    pos = split_str.find(delimiter, i);
    if (pos < size) {
      std::string s = split_str.substr(i, pos - i);
      split_result.push_back(s);
      i = pos + delimiter.size() - 1;
    }
  }
}

bool loadinitMapYAML(const std::string &file_name, std::string &map_id)
{
  std::ifstream fin(file_name.c_str());
  if (fin.fail()) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "mapID param could not open " << file_name.c_str());
    return false;
  }


  try {
    YAML::Node loaded = YAML::LoadFile(file_name);
    if (!loaded) {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "Read YAML Faild. data is not yaml format...");
      return false;
    }
    map_id = loaded["agv_map_name"].as<std::string>();
  }
  catch (const YAML::Exception& e) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("slam_node"), "Read " << file_name.c_str() << " error is " << e.what());
    map_id = "";
    return false;
  }

  return true;
}

int get_file_md5(const std::string &file_name, std::string &md5_value)
{
  md5_value.clear();

  std::ifstream file(file_name.c_str(), std::ifstream::binary);
  if (!file)
  {
    return -1;
  }

  MD5_CTX md5Context;
  MD5_Init(&md5Context);

  char buf[1024 * 16];
  while (file.good()) {
    file.read(buf, sizeof(buf));
    MD5_Update(&md5Context, buf, file.gcount());
  }

  unsigned char result[MD5_DIGEST_LENGTH];
  MD5_Final(result, &md5Context);

  char hex[35];
  memset(hex, 0, sizeof(hex));
  for (int i = 0; i < MD5_DIGEST_LENGTH; ++i)
  {
    sprintf(hex + i * 2, "%02x", result[i]);
  }
  hex[32] = '\0';
  md5_value = std::string(hex);

  return 0;
}


}
